﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using CalcularGanhosPolos;
using MathWorks.MATLAB.NET.Arrays;
using MathWorks.MATLAB.NET.Utility;

namespace TankController.Dominio
{
    public class SeguidorReferencia : AbstractControlador
    {
        protected double ganho;
        protected double k1;
        protected double k2;
        protected double k3;
        protected Complexo p1;
        protected Complexo p2;
        protected Complexo p3;
        //protected static Matriz g;
        //protected static Matriz h;
        //protected static Matriz c;
        //protected static Matriz i3;
        //protected static Matriz kChapeuParcial;
        //protected static Matriz gChapeu;
        //protected static Matriz cLine;
        //protected static Matriz invAckerman;
        protected IEntrada referencia;
        protected ISinal sinalReferencia = new Sinal("Referência", TipoSinal.Referencia);
        protected ISinal sinalErro = new Sinal("Erro", TipoSinal.Erro);

        //protected double valorSaidaAnterior = 0;
        protected double integralErro = 0;

        private static CalcularGanhosPolos.CalcularGanhosPolos cgp;

        static SeguidorReferencia()
        {
            //MontarMatrizes();
            cgp = new CalcularGanhosPolos.CalcularGanhosPolos();
        }

        public SeguidorReferencia()
        {

        }

        public SeguidorReferencia(double k1, double k2, double k3)
            : this()
        {
            this.k1 = k1;
            this.k2 = k2;
            this.k3 = k3;
        }

        public SeguidorReferencia(IPlanta planta, double k1, double k2, double k3, int periodo)
            : this(k1, k2, k3)
        {
            this.planta = planta;
            this.periodo = periodo;
        }

        protected override void Controlar()
        {
            double valorReferencia = referencia.GetValorAmplitude(tempoAtual);
            double valorProcesso = planta.Ler() * planta.FatorConversao;
            double valorProcessoAuxiliar = planta.LerSensorSecundario() * planta.FatorConversao;
            double valorErro = valorReferencia - valorProcesso;
            double valorSaida = 0;
            double valorSaidaCorrigido = 0;

            valorSaida = CalcularSaida(valorErro/* * planta.FatorConversao*/, valorProcesso, valorProcessoAuxiliar) / planta.FatorConversao;
            //valorSaida = CalcularSaida(valorReferencia, valorProcesso, valorProcessoAuxiliar) / planta.FatorConversao;

            //valorSaidaCorrigido = ManterNivelSeguranca(valorProcessoAuxiliar, ManterNivelSeguranca(valorProcesso, valorSaida));
            valorSaidaCorrigido = ManterNivelSeguranca(valorProcessoAuxiliar, valorSaida);
            //valorSaidaCorrigido = ManterNivelSeguranca(valorProcesso, valorSaida);

            planta.Escrever(valorSaidaCorrigido);

            //valorSaidaAnterior = valorSaidaCorrigido;

            sinalProcessoSensor2.AdicionarPonto(tempoAtual, valorProcesso);
            sinalProcessoSensor1.AdicionarPonto(tempoAtual, valorProcessoAuxiliar);

            referencia.GerarPontoSinal(tempoAtual, valorReferencia, sinalReferencia);
            sinalErro.AdicionarPonto(tempoAtual, valorErro);
            sinalSaida.AdicionarPonto(tempoAtual, valorSaidaCorrigido);
        }

        protected double CalcularSaida(double valorErro, double valorProcesso, double valorProcessoAuxiliar)
        {
            integralErro += valorErro; //*((double)periodo / 1000);

            return (integralErro * k3) - (valorProcessoAuxiliar * k1 + valorProcesso * k2);

            /*double t1 = (K1 * (1 - g[0, 0]) + (-K2 - K3) * g[1, 0]) * valorProcessoAuxiliar;
            double t2 = (K2 * (1 - g[1, 1]) - K1 * g[0, 1] - K3 * g[1, 1]) * valorProcesso;
            double t3 = (1 - K1 * h[0, 0] - K2 * h[1, 0] - K3 * h[1, 0]) * valorSaidaAnterior;
            double t4 = k3 * valorErro;

            return t1 + t2 + t3 + t4;*/
        }

        /*protected void ConfigurarSinais(TipoPosicaoControlador tipoPosicao)
        {
            if (Global.TipoConfiguracao == TipoConfiguracao.ConfiguracaoC)
            {
                if (tipoPosicao == TipoPosicaoControlador.Controlador1)
                {
                    sinalProcessoSensor1.Descricao = "Nível Tanque 2";
                    sinalReferencia.Descricao = "Referência Controlador 1";
                    sinalErro.Descricao = "Erro Controlador 1";
                    sinalSaida.Descricao = "Saída Controlador 1";

                    sinalProcessoSensor1.Tipo = TipoSinal.Resposta;
                }
                else if (tipoPosicao == TipoPosicaoControlador.Controlador2)
                {
                    sinalProcessoSensor1.Descricao = "Nível Tanque 1";
                    sinalReferencia.Descricao = "Referência Controlador 2";
                    sinalErro.Descricao = "Erro Controlador 2";
                    sinalSaida.Descricao = "Saída Controlador 2";

                    sinalProcessoSensor1.Tipo = TipoSinal.Nivel;
                }
                else
                {
                    throw new ApplicationException("Tipo de posição do controlador inválida.");
                }
            }
        }*/

        public override IList<ISinal> GetSinaisAltura()
        {
            IList<ISinal> sinalList = new List<ISinal>();

            sinalList.Add(sinalReferencia);
            sinalList.Add(sinalProcessoSensor1);
            sinalList.Add(sinalProcessoSensor2);

            return sinalList;
        }

        public override IList<ISinal> GetSinaisTensao()
        {
            IList<ISinal> sinalList = new List<ISinal>();

            sinalList.Add(sinalErro);
            sinalList.Add(sinalSaida);

            return sinalList;
        }

        public void SetGanhos(double k1, double k2, double k3)
        {
            this.k1 = k1;
            this.k2 = k2;
            this.k3 = k3;

            Complexo[] polos = CalcularPolosDeGanhos(k1, k2, k3);

            this.p1 = polos[0];
            this.p2 = polos[1];
            this.p3 = polos[2];
        }

        public void SetPolos(Complexo p1, Complexo p2, Complexo p3)
        {
            this.p1 = p1;
            this.p2 = p2;
            this.p3 = p3;

            double[] ganhos = CalcularGanhosDePolos(p1, p2, p3);

            this.k1 = ganhos[0];
            this.k2 = ganhos[1];
            this.k3 = ganhos[2];
        }

        protected override void Reiniciar()
        {
            this.integralErro = 0;
            //this.valorSaidaAnterior = 0;
        }

        public static double[] CalcularGanhosDePolos(Complexo p1, Complexo p2, Complexo p3)
        {
            MWNumericArray p1Array = new MWNumericArray(p1.Real, p1.Imaginario);
            MWNumericArray p2Array = new MWNumericArray(p2.Real, p2.Imaginario);
            MWNumericArray p3Array = new MWNumericArray(p3.Real, p3.Imaginario);

            MWNumericArray arrayGanhos = (MWNumericArray)cgp.ObterGanhosDePolos(p1Array, p2Array, p3Array);

            double k1 = Math.Round(arrayGanhos[1].ToScalarDouble(), 4);
            double k2 = Math.Round(arrayGanhos[2].ToScalarDouble(), 4);
            double k3 = Math.Round(arrayGanhos[3].ToScalarDouble(), 4);

            return new double[] { k1, k2, k3 };
        }

        public static Complexo[] CalcularPolosDeGanhos(double k1, double k2, double k3)
        {
            MWNumericArray arrayPolos = (MWNumericArray)cgp.ObterPolosDeGanhos(k1, k2, k3);

            Complexo p1 = new Complexo();
            Complexo p2 = new Complexo();
            Complexo p3 = new Complexo();

            if (arrayPolos[1].IsComplex)
            {
                p1.Real = Math.Round((double)arrayPolos[1].ToArray(MWArrayComponent.Real).GetValue(0, 0), 4);
                p1.Imaginario = Math.Round((double)arrayPolos[1].ToArray(MWArrayComponent.Imaginary).GetValue(0, 0), 4);
            }
            else
            {
                p1.Real = Math.Round(arrayPolos[1].ToScalarDouble(), 4);
            }

            if (arrayPolos[2].IsComplex)
            {
                p2.Real = Math.Round((double)arrayPolos[2].ToArray(MWArrayComponent.Real).GetValue(0, 0), 4);
                p2.Imaginario = Math.Round((double)arrayPolos[2].ToArray(MWArrayComponent.Imaginary).GetValue(0, 0), 4);
            }
            else
            {
                p2.Real = Math.Round(arrayPolos[2].ToScalarDouble(), 4);
            }

            if (arrayPolos[3].IsComplex)
            {
                p3.Real = Math.Round((double)arrayPolos[3].ToArray(MWArrayComponent.Real).GetValue(0, 0), 4);
                p3.Imaginario = Math.Round((double)arrayPolos[3].ToArray(MWArrayComponent.Imaginary).GetValue(0, 0), 4);
            }
            else
            {
                p3.Real = Math.Round(arrayPolos[3].ToScalarDouble(), 4);
            }

            return OrdenarPolos(ref p1, ref p2, ref p3);
        }

        private static Complexo[] OrdenarPolos(ref Complexo p1, ref Complexo p2, ref Complexo p3)
        {
            if (p1.Real == p3.Real)
            {
                if (p1.Imaginario > p3.Imaginario)
                {
                    return new Complexo[] { p1, p3, p2 };
                }
                else
                {
                    return new Complexo[] { p3, p1, p2 };
                }
            }
            else if (p2.Real == p3.Real)
            {
                if (p2.Imaginario > p3.Imaginario)
                {
                    return new Complexo[] { p2, p3, p1 };
                }
                else
                {
                    return new Complexo[] { p3, p2, p1 };
                }
            }
            else
            {
                if (p1.Imaginario > p2.Imaginario)
                {
                    return new Complexo[] { p1, p2, p3 };
                }
                else
                {
                    return new Complexo[] { p2, p1, p3 };
                }
            }
        }

        //private static void MontarMatrizes()
        //{
        //    g = new Matriz(2, 2);
        //    h = new Matriz(2, 1);
        //    c = new Matriz(1, 2);
        //    i3 = new Matriz(3, 3);
        //    kChapeuParcial = new Matriz(1, 3);
        //    gChapeu = new Matriz(3, 3);
        //    cLine = new Matriz(1, 3);
        //    invAckerman = new Matriz(3, 3);

        //    g[0, 0] = 0.993620436379149;
        //    g[0, 1] = 0;
        //    g[1, 0] = 0.006359170792827;
        //    g[1, 1] = 0.993620436379149;

        //    h[0, 0] = 0.047826791020067;
        //    h[1, 0] = 0.000152882482596;

        //    c[0, 0] = 0;
        //    c[0, 1] = 1;

        //    i3[0, 0] = 1;
        //    i3[1, 1] = 1;
        //    i3[2, 2] = 1;

        //    kChapeuParcial[0, 0] = -10.510291280533000;
        //    kChapeuParcial[0, 1] = 3287.973194179446;
        //    kChapeuParcial[0, 2] = 0;

        //    gChapeu[0, 0] = 0.993620436379149;
        //    gChapeu[0, 1] = 0;
        //    gChapeu[0, 2] = 0.047826791020067;
        //    gChapeu[1, 0] = 0.006359170792827;
        //    gChapeu[1, 1] = 0.993620436379149;
        //    gChapeu[1, 2] = 0.000152882482596;
        //    gChapeu[2, 0] = 0;
        //    gChapeu[2, 1] = 0;
        //    gChapeu[2, 2] = 0;

        //    cLine[0, 0] = 0;
        //    cLine[0, 1] = 0;
        //    cLine[0, 2] = 1;

        //    invAckerman[0, 0] = -0.501066665210300;
        //    invAckerman[0, 1] = 155.750533332969200;
        //    invAckerman[0, 2] = 1;
        //    invAckerman[1, 0] = 0;
        //    invAckerman[1, 1] = -1;
        //    invAckerman[1, 2] = 1;
        //    invAckerman[2, 0] = 20.841946366687300;
        //    invAckerman[2, 1] = 20.775394191976000;
        //    invAckerman[2, 2] = 0.133388912046700;
        //}

        public override double Ganho
        {
            get { throw new NotSupportedException("Parâmetro não suportado."); }
            set { throw new NotSupportedException("Parâmetro não suportado."); }
        }
        public override String Descricao
        {
            get { return "Seguidor de Referência"; }
        }
        public double K1
        {
            get { return k1; }
        }
        public double K2
        {
            get { return k2; }
        }
        public double K3
        {
            get { return k3; }
        }
        public Complexo P1
        {
            get { return p1; }
        }
        public Complexo P2
        {
            get { return p2; }
        }
        public Complexo P3
        {
            get { return p3; }
        }
        public IEntrada Referencia
        {
            get { return referencia; }
            set { referencia = value; }
        }
        public ISinal SinalReferencia
        {
            get { return sinalReferencia; }
            set { sinalReferencia = value; }
        }
        public ISinal SinalErro
        {
            get { return sinalErro; }
            set { sinalErro = value; }
        }
    }
}
